//
// Created by Administrator on 2019\11\1 0001.

#ifndef TCLCAPDEPTH_TCLSTEREODEPTHALGUTILS_H
#define TCLCAPDEPTH_TCLSTEREODEPTHALGUTILS_H


#define CV_CPU_HAS_SUPPORT_NEON 1
#include "opencv2/core/hal/intrin.hpp"

#include "stdlib.h"
#include <cmath>

#include "native_debug.h"

#include <sys/stat.h>
#include <sys/types.h>
#include <errno.h>
#include <thread>
#include <chrono>

#include <iostream>
#include <cmath>
#include <opencv2/opencv.hpp>
#include "opencv2/ximgproc/edge_filter.hpp"
#include "opencv2/ximgproc/disparity_filter.hpp"
#include "opencv2/ximgproc/weighted_median_filter.hpp"
#include <opencv2/xfeatures2d.hpp>

#include "tcl_dualcam_def.h"

using namespace cv;
using namespace std;
using namespace cv::ximgproc;

#ifdef _WIN32
#define FAIL_EXIT(code) {system("pause");exit(code);}
#define PAUSE() {system("pause");}
#else
#define FAIL_EXIT(code) {exit(code);}
#define PAUSE() {}
#endif

namespace tcl_depth_video {

	enum ThreadState {
		THREAD_STATE_PLAY = 1,
		THREAD_STATE_WAIT = 2,
		THREAD_STATE_QUIT = 3
	};

	enum RotateCode {
		ROTATE_CODE_0 = 0,
		ROTATE_CODE_90 = 1,
		ROTATE_CODE_180 = 2,
		ROTATE_CODE_270 = 3
	};

	void VisualizeOpticalFlow(
		cv::Mat & output,
		const std::vector<cv::Point2f> & src,
		const std::vector<cv::Point2f> & dst);

	void Erode_v3(
		cv::Mat & o_,
		const cv::Mat & i_,
		const cv::Mat & rgb,
		int r,
		uint8_t threshold
		);

	void Dilate_v3(
		cv::Mat & o_,
		const cv::Mat & i_,
		const cv::Mat & rgb,
		int r,
		uint8_t threshold
		);

	int SmoothDisparity(cv::Mat & o_,
		const cv::Mat & i_,
		const cv::Mat & edge,
		bool use_mask,
		int min_support,
		ThreadState* thread_state
		);

	void GetFullEdges(cv::Mat & o_, const cv::Mat & i_, int thres_selc, int thres_conn);

	void GetFullEdgesYUV(
		cv::Mat & o_,
		const cv::Mat & Y,
		const cv::Mat & U,
		const cv::Mat & V,
		int thres_selc,
		int thres_conn
		);

	void FindConnectedDomain(
		cv::Mat & domain_map,
		std::vector<int> & counts,
		cv::Rect & visible_zone,
		const cv::Mat & edge);

	void visualize_domains(cv::Mat & rgb, const cv::Mat & domains, int num_domain);

	void ColorBasedFilter(
		cv::Mat & o_,
		const cv::Mat & i_,
		const cv::Mat & rgb,
		int r
		);

	class Erode_v3_fastest_class;

	void Erode_v3_fastest(
		cv::Mat & o_,
		const cv::Mat & i_,
		const cv::Mat & rgb,
		int r,
		int threads_num,
		uint8_t threshold = 255
		);

	class Dilate_v3_fastest_class;


	void Dilate_v3_fastest(
		cv::Mat & o_,
		const cv::Mat & i_,
		const cv::Mat & rgb,
		int r,
		int threads_num,
		uint8_t threshold = 255
		);

	void FillBlackBorder(cv::Mat & disp_pri, int std_max_disp, int win_size);

	void Histogram(cv::Mat & hist, const cv::Mat & depth, int hist_size);

	void VisualizeHistogram(cv::String title, cv::Mat & hist);

	// for static stereo rectification
	void StaticStereoRectify(
		const cv::Mat & M1,
		const cv::Mat & D1,
		const cv::Mat & M2,
		const cv::Mat & D2,
		const cv::Mat & R,
		const cv::Mat & T,
		const cv::Size & size,
		cv::Mat & map11,
		cv::Mat & map12,
		cv::Mat & map21,
		cv::Mat & map22,
		cv::Mat & imap1,
		cv::Mat & imap2,
		cv::Mat & Q
		);

	int RowAlign_grid(
		cv::Mat & im2_aligned, // aligned aux
		const cv::Mat & im1, // main
		const cv::Mat & im2, // aux
		int num_feat,
		int max_iter,
		float res_level,
		int num_worst
		);


	int RowAlign_grid_tws(
		cv::Mat & im2_aligned, // aligned aux
		const cv::Mat & im1, // main
		const cv::Mat & im2, // aux
		int num_feat,
		int max_iter,
		float res_level,
		int num_worst,
		cv::Mat & map_x,
		cv::Mat & map_y,
		bool update_map,
		ThreadState * thread_state,
		bool & success_flag,
		float & real_max_disp,
		std::vector<float> & disp_spa,
		std::vector<cv::Point2f> & coord_spa
		);

	int RowAlign_xiang(
		float & v_x, // velocity in x
		float & v_y, // velocity in y
		cv::Mat & im1_aligned, // aligned main
		cv::Mat & im2_aligned, // aligned aux
		const cv::Mat & im1, // main
		const cv::Mat & im2, // aux
		float res_level,
		int num_worst,
		ThreadState * thread_state,
		RotateCode & rotate_code,
		float & real_max_disp
		);

	void HistBasedFilter(cv::Mat & im_disp, float min_support, bool ignore_zero);

	void HistBasedPeakFilter(cv::Mat & im_disp, float min_support, int min_disp);

	// dump images
	void DumpImage(const cv::Mat & im_,
		const cv::String & path,
		const cv::String & postfix,
		const cv::String & uid);

	// fill the rest black holes with color similarity
	void FillBlackHoles(cv::Mat & io_, const cv::Mat & guide);

	void GetOcclusionRegions(const cv::Mat & disp_left,
		const cv::Mat & disp_right,
		cv::Mat & output,
		int threshold,
		bool right_first = false);

	void GetReconstructError(const cv::Mat & disp_left,
		const cv::Mat & disp_right,
		const cv::Mat & im_left,
		const cv::Mat & im_right,
		cv::Mat & err_mask,
		cv::Mat & im_rec,
		bool right = false);

	void CorrectDisparityLRC_v1(cv::Mat & disp,
		const cv::Mat & mask,
		int max_disp,
		bool right = false);

	void CorrectDisparityLRC_v2(cv::Mat & disp_left,
		cv::Mat & disp_right,
		const cv::Mat & mask,
		bool right = false);

	//void GradSmooth(cv::Mat &depth_out, int thresh, int radius, int local_diff);

	void Disparity2Depth(cv::Mat &disp);

	void Disparity2Depth_v2(cv::Mat &disp);

	void Depth2Disparity(cv::Mat &depth);

	void ChangeSaturation(cv::Mat &in_out, int degree);

	bool CheckMotion(const cv::Mat & im,
		const cv::Mat & im_moved,
		float threshold);

	int GuessMotion(float &dx, float &dy,
		const cv::Mat & im, const cv::Mat & im_moved,
		ThreadState* thread_state);

	void DumpMatrix(const cv::Mat & mat, const cv::String & name);

	void ScaleDownAndCenterCrop(cv::Mat & o_,
		const cv::Mat & i_,
		double scale,
		const cv::Size & base,
		const cv::Size & crop);

	void UpdateCameraMatrix(cv::Mat cm, double scale, const cv::Size & crop);


	void GenerateInverseMap(
		cv::Mat & imap1,
		cv::Mat & imap2,
		const cv::Size size,
		const cv::Mat & M,
		const cv::Mat & D,
		const cv::Mat & R,
		const cv::Mat & P);

	void GetRightPoint(std::vector<cv::Point2f> &rightVec,
		std::vector<cv::Point2f> &leftVec,
		std::vector<cv::Point2f> &rightVecOut,
		std::vector<cv::Point2f> &leftVecOut,
		std::vector<uchar> &status);

	void read_yuv_file(uint8_t * y_plane,
		uint8_t * uv_plane,
		uint64_t y_size,
		uint64_t uv_size,
		const char * file);

	LPTCL_IMAGE create_yuv_image(int w, int h, int pitch,
		uint32_t mode = TCL_PAF_NV21);

	void load_yuv_data(LPTCL_IMAGE yuv, const char * file);

	void free_yuv_image(LPTCL_IMAGE yuv);

	void copy_yuv_image(LPTCL_IMAGE dst, LPTCL_IMAGE src);

	void sample_Y_3x3(cv::Mat & dst, const LPTCL_IMAGE src);

	void sample_Y_4x4(cv::Mat & dst, const LPTCL_IMAGE src);

	void sample_Y_2x2(cv::Mat & dst, const LPTCL_IMAGE src);

	void sample_NV21_2x2(cv::Mat & dst_Y, cv::Mat & dst_U, cv::Mat & dst_V, const LPTCL_IMAGE src);

	void sample_NV12_2x2(cv::Mat & dst_Y, cv::Mat & dst_U, cv::Mat & dst_V, const LPTCL_IMAGE src);

	void YUV2RGB_simple(cv::Mat & rgb, cv::Mat & Y, const cv::Mat & U, const cv::Mat & V);

	void FillBlackHoles_Gray(cv::Mat & io_, const cv::Mat & guide);

	void StrokeGridLines(cv::Mat & dis,
		const cv::Mat & curr,
		const std::vector<uint8_t> & disp,
		const std::vector<cv::Point2i> & pts);

};

#endif //TCLCAPDEPTH_TCLSTEREODEPTHALGUTILS_H


